﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

using Drifting.Foundation;
using Drifting.Graphics.EffectObjects;
using Drifting.Graphics.Visualizers;
using Drifting.Graphics.PhysicalObjects.Vehicles;
using Drifting.MotionPlanning.MultiThreading.Planners;
using Drifting.MotionPlanning.Primitives;
using Drifting.MotionPlanning.Systems;

namespace Drifting.Scenes
{
    public class SST_Drift8D : Basic
    {
        SST sst;
        E92Test m3;

        public SST_Drift8D(ref Matrix view, ref Matrix projection, GraphicsDevice device, Camera camera, Game1 game)
            : base(ref view, ref projection, device, camera, game)
        {
            camera.Camera_InitView(40, 10, 20, 0, 0, 0, (float)(Math.PI / 4), ref view, ref projection);

            BaseSystem drift8D = new Drift8D();
            Skin_Drift8D rallyCar_visual = new Skin_Drift8D(new List<State>(), device);

            double[] q_zero = new double[] { -9.9, -5, 0, 14f, 0, 0, 30, 30 };
            sst = new SST(
                drift8D,
                q_zero,
                TextAssistant.LoadFile("../../../MotionPlanning/Resources/SST_Drift8D.kdl"),
                inXgoal,
                device,
                rallyCar_visual);

            m3 = new E92Test(game);

            sst.AgentNumber = 5;
            sst.Viewable = true;
            sst.Start(100000000); // 100 million
        }

        override public bool inXgoal(double[] q)
        {
            if (q[0] > -4)
                return false;

            if (q[1] < 0)
                return false;

            if (Math.Abs(q[2]) < 2.7)
                return false;

            return true;
        }

        override public void Draw(Matrix camera, Matrix projection)
        {
            base.Draw(camera, projection);

            sst.Skin.Draw(camera, projection, sst.x_solution);

            // draw solution
            if (sst.x_solution != null)
            {
                State x_i = sst.x_solution;
                while (x_i != null)
                {
                    double[] q = x_i.Q;
                    if (x_i.ParentEdge != null)
                    {
                        m3.Draw(camera, projection, (float)q[0], (float)q[1], (float)q[2], (float)x_i.ParentEdge.control[0]);
                    }

                    if (x_i.ParentEdge == null)
                        break;
                    x_i = x_i.ParentEdge.entry;
                }
            }
        }

        override public void GetStatistics(ref List<string> info)
        {
            sst.GetStatistics(ref info);
        }

        override public void Terminate()
        {
            sst.Terminate();
        }
    }
}
